/**
 * @file ros_control_driver.cpp
 * @author circleup (circleup@foxmail.com)
 * @brief 数据处理，控制函数
 * @version 2.8
 * @date 2020-07-08
 * 
 * @copyright Copyright (c) 2020
 * 
 */

#include "ros_control_driver.h"
#include "ros_can_driver.h"

#include <math.h>
#include "ros/ros.h"

ros_car::car_status_frame serial_msg;

void SetVCUDefaultValue(ros_car::VCU_frame *dest_value)
{
  VCU_FRAME_DATA VCU_FRAME_DATA;
  memcpy(&VCU_FRAME_DATA, &(dest_value->arryData), sizeof(VCU_FRAME_DATA));

  VCU_FRAME_DATA.VCU_GEAR_REQ = 0x01;
  VCU_FRAME_DATA.VCU_BREAK_REQ = 0x01;
  VCU_FRAME_DATA.VCU_STR_REQ = 0x01;
  VCU_FRAME_DATA.VCU_PED_REQ = 0x01;
  VCU_FRAME_DATA.VCU_MODE = 0x01;
  memcpy(&(dest_value->arryData), &VCU_FRAME_DATA, sizeof(VCU_FRAME_DATA));
}

void SetVCUPedValue(ros_car::VCU_frame *dest_value, uint8_t src_value)
{
  VCU_FRAME_DATA VCU_FRAME_DATA;
  memcpy(&VCU_FRAME_DATA, &(dest_value->arryData), sizeof(VCU_FRAME_DATA));

  if(src_value > 0x64)
  {
    src_value = 0x64;
  }
  VCU_FRAME_DATA.VCU_PED_VALUE = src_value;
  memcpy(&(dest_value->arryData), &VCU_FRAME_DATA, sizeof(VCU_FRAME_DATA));
}

void SetVCUSTRAngleValue(ros_car::VCU_frame *dest_value, int16_t src_value)
{
  if(src_value > 300)
  {
    src_value = 300;
  }
  if(src_value < -300)
  {
    src_value = -300;
  }

  uint16_t temp = (src_value + 550) * 10;
  VCU_FRAME_DATA VCU_FRAME_DATA;
  memcpy(&VCU_FRAME_DATA, &(dest_value->arryData), sizeof(VCU_FRAME_DATA));

  VCU_FRAME_DATA.VCU_STR_ANGLE_H = (temp >> 6) & 0x00FF;
  VCU_FRAME_DATA.VCU_STR_ANGLE_L = temp & 0x003F;
  memcpy(&(dest_value->arryData), &VCU_FRAME_DATA, sizeof(VCU_FRAME_DATA));
}

void SetVCUBreakValue(ros_car::VCU_frame *dest_value, uint8_t src_value)
{
  VCU_FRAME_DATA VCU_FRAME_DATA;
  memcpy(&VCU_FRAME_DATA, &(dest_value->arryData), sizeof(VCU_FRAME_DATA));
  
  if(src_value > 0x64)
  {
    src_value = 0x64;
  }
  VCU_FRAME_DATA.VCU_BREAK_VALUE = src_value;
  memcpy(&(dest_value->arryData), &VCU_FRAME_DATA, sizeof(VCU_FRAME_DATA));
}

void SetVCUGearValue(ros_car::VCU_frame *dest_value, uint8_t src_value)
{
  VCU_FRAME_DATA VCU_FRAME_DATA;
  memcpy(&VCU_FRAME_DATA, &(dest_value->arryData), sizeof(VCU_FRAME_DATA));

  if(src_value > 0x02)
  {
    src_value = 0x01;
  }
  VCU_FRAME_DATA.VCU_GEAR_VALUE = src_value & 0x03;
  memcpy(&(dest_value->arryData), &VCU_FRAME_DATA, sizeof(VCU_FRAME_DATA));
}

double GetDistance(double lat1, double lon1, double lat2, double lon2) 
{
  double a = 6378137, b = 6356752.314245, f = 1 / 298.257223563;
  double L = (lon2 - lon1) * PI / 180;


  double U1 = atan((1 - f) * tan((lat1) * PI / 180));
  double U2 = atan((1 - f) * tan((lat2) * PI / 180));
  double sinU1 = sin(U1), cosU1 = cos(U1);
  double sinU2 = sin(U2), cosU2 = cos(U2);
  double cosSqAlpha;
  double sinSigma;
  double cos2SigmaM;
  double cosSigma;
  double sigma;

  double lambda = L, lambdaP, iterLimit = 100;
  do 
  {
    double sinLambda = sin(lambda), cosLambda = cos(lambda);
    sinSigma = sqrt(  (cosU2 * sinLambda)
            * (cosU2 * sinLambda)
              + (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda)
                * (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda)
              );
    if (sinSigma == 0) 
    {
      return 0;
    }

    cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda;
    sigma = atan2(sinSigma, cosSigma);
    double sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma;
    cosSqAlpha = 1 - sinAlpha * sinAlpha;
    cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha;

    double C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha));
    lambdaP = lambda;
    lambda =   L + (1 - C) * f * sinAlpha  
          *   (sigma + C * sinSigma  
            *   (cos2SigmaM + C * cosSigma
              *   (-1 + 2 * cos2SigmaM * cos2SigmaM)
              )
            );
    
  } while (abs(lambda - lambdaP) > 1e-12 && --iterLimit > 0);

  if (iterLimit == 0) 
  {
    return 0;
  }

  double uSq = cosSqAlpha * (a * a - b * b) / (b * b);
  double A = 1 + uSq / 16384
      * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)));
  double B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)));
  double deltaSigma = 
        B * sinSigma
          * (cos2SigmaM + B / 4
            * (cosSigma 
              * (-1 + 2 * cos2SigmaM * cos2SigmaM) - B / 6 * cos2SigmaM
                * (-3 + 4 * sinSigma * sinSigma)
                  * (-3 + 4 * cos2SigmaM * cos2SigmaM)));
  
  double s = b * A * (sigma - deltaSigma);
    
  return s;
}

void LonLat2UTM(double longitude, double latitude, double& UTME, double& UTMN)
{
  double lat = latitude;
  double lon = longitude;

  double kD2R = PI / 180.0;
  double ZoneNumber = floor((lon - 1.5) / 3.0) + 1;
  double L0 = ZoneNumber * 3.0;

  double a = 6378137.0;
  double F = 298.257223563;
  double f = 1 / F;
  double b = a * (1 - f);
  double ee = (a * a - b * b) / (a * a);
  double e2 = (a * a - b * b) / (b * b);
  double n = (a - b) / (a + b); 
  double n2 = (n * n); 
  double n3 = (n2 * n); 
  double n4 = (n2 * n2); 
  double n5 = (n4 * n);
  double al = (a + b) * (1 + n2 / 4 + n4 / 64) / 2.0;
  double bt = -3 * n / 2 + 9 * n3 / 16 - 3 * n5 / 32.0;
  double gm = 15 * n2 / 16 - 15 * n4 / 32;
  double dt = -35 * n3 / 48 + 105 * n5 / 256;
  double ep = 315 * n4 / 512;
  double B = lat * kD2R;
  double L = lon * kD2R;
  L0 = L0 * kD2R;
  double l = L - L0; 
  double cl = (cos(B) * l); 
  double cl2 = (cl * cl); 
  double cl3 = (cl2 * cl); 
  double cl4 = (cl2 * cl2); 
  double cl5 = (cl4 * cl); 
  double cl6 = (cl5 * cl); 
  double cl7 = (cl6 * cl); 
  double cl8 = (cl4 * cl4);
  double lB = al * (B + bt * sin(2 * B) + gm * sin(4 * B) + dt * sin(6 * B) + ep * sin(8 * B));
  double t = tan(B); 
  double t2 = (t * t); 
  double t4 = (t2 * t2); 
  double t6 = (t4 * t2);
  double Nn = a / sqrt(1 - ee * sin(B) * sin(B));
  double yt = e2 * cos(B) * cos(B);
  double N = lB;
  N = N + t * Nn * cl2 / 2;
  N = N + t * Nn * cl4 * (5 - t2 + 9 * yt + 4 * yt * yt) / 24;
  N = N + t * Nn * cl6 * (61 - 58 * t2 + t4 + 270 * yt - 330 * t2 * yt) / 720;
  N = N + t * Nn * cl8 * (1385 - 3111 * t2 + 543 * t4 - t6) / 40320;
  double E = Nn * cl;
  E = E + Nn * cl3 * (1 - t2 + yt) / 6;
  E = E + Nn * cl5 * (5 - 18 * t2 + t4 + 14 * yt - 58 * t2 * yt) / 120;
  E = E + Nn * cl7 * (61 - 479 * t2 + 179 * t4 - t6) / 5040;
  E = E + 500000;
  N = 0.9996 * N;
  E = 0.9996 * (E - 500000.0) + 500000.0;

  UTME = E;
  UTMN = N;
}

double GPSAngleCalculateUTM(double& UTME1, double& UTMN1, double& UTME2, double& UTMN2)
{
  double x_err  = UTME2 - UTME1;
  double y_err  = UTMN2 - UTMN1;
  
  if (x_err > 0)
  {
    if (y_err > 0)
    {
      return (atan2f(x_err, y_err)) * 180 / PI;
    }
    else if ( y_err < 0)
    {
      return (atan2f(-y_err, x_err) * 180 / PI) + 90;
    }
    else
    {
      return 90;
    }
    
  }
  else if (x_err < 0)
  {
    if (y_err > 0)
    {
      return ((atan2f(y_err, -x_err)) * 180 / PI) + 270;
    }
    else if ( y_err < 0)
    {
      return (atan2f(-x_err, -y_err) * 180 / PI) + 180;
    }
    else
    {
      return 290;
    }
  }
  else
  {
    if (y_err > 0)
    {
      return 0;
    }
    else if (y_err < 0)
    {
      return 180;
    }
    else
    {
      return -1;
    }
  }
}

double GPSAngleCalculateUTMFromGPS(double longitude1, double latitude1, double longitude2, double latitude2)
{
  double posx1, posy1, posx2, posy2;
  LonLat2UTM(longitude1, latitude1, posx1, posy1);
  LonLat2UTM(longitude2, latitude2, posx2, posy2);

  return GPSAngleCalculateUTM(posx1, posy1, posx2, posy2);
}

double GetDistanceUTM(double UTME1, double UTMN1, double UTME2, double UTMN2)
{
  return sqrt(
    pow((UTME1 - UTME2), 2) +
    pow((UTMN1 - UTMN2), 2)
  );
}

double GetDistanceUTMFromGPS(double longitude1, double latitude1, double longitude2, double latitude2)
{
  double posx1, posy1, posx2, posy2;
  LonLat2UTM(longitude1, latitude1, posx1, posy1);
  LonLat2UTM(longitude2, latitude2, posx2, posy2);

  return GetDistanceUTM(posx1, posy1, posx2, posy2);
}